#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "LED.h"
#include "Key.h"
#include "AD.h"
#include "Motor.h"
#include "Timer.h"
#include "Encoder.h"
#include "Serial.h"
#include <math.h>
#include "AD_JD.h"
#include "PID.h"

uint8_t KeyNum;
uint8_t RunState;
uint16_t angle;
#define CENTER_ANGLE 2010
#define CENTER_RANGE 500


#define Time_CNT 100
#define Angle_PWM 35


uint8_t Count, Count0, Count1, Count2;
float Speed, Location;

PID_t AnglePID = {
    .Target = CENTER_ANGLE,
    
    .Kp = 0.3,
    .Ki = 0.01, // 加入i可以使效果更好，角度相差不大所以累积的误差很小，不会导致超调
    .Kd = 0.4,
    
    .OutMax = 100,
    .OutMin = -100,
};

PID_t LocationPID = {
    .Target = 0,
    
    .Kp = 0.4,
    .Ki = 0,
    .Kd = 4,
    
    .OutMax = 100,
    .OutMin = -100,
};

/*测试*/
int main(void)
{
	/*模块初始化*/
    RP_Init();      // 电位器驱动
    AD_JD_Init();   // 角度传感器
    Key_Init();
    LED_Init();
    OLED_Init();
    Motor_Init();
    Serial_Init();
    Encoder_Init();
    Timer_Init();   // 定时器
    
	while (1)
	{ 
        KeyNum = Key_GetNum();
        if (KeyNum == 1)
        {
            if (RunState == 0)
                RunState = 21; // 起摆，脱离盲区
            else 
                RunState = 0;
        }
        if (KeyNum == 2)
        {
            LocationPID.Target += 408;
            if (LocationPID.Target >= 4080)
            {
                LocationPID.Target = 4080;
            }
        }
        if (KeyNum == 2)
        {
            LocationPID.Target -= 408;
            if (LocationPID.Target <= -4080)
            {
                LocationPID.Target = -4080;
            }
        }
        
        if (RunState)
        {
            LED_ON();
        }
        else
        {
            LED_OFF();
        }
        
        
//        AnglePID.Kp = RP_GetValue(1) / 4095.0 * 1;
//        AnglePID.Ki = RP_GetValue(2) / 4095.0 * 1;
//        AnglePID.Kd = RP_GetValue(3) / 4095.0 * 1;
        
        OLED_Printf(0, 0, OLED_6X8, "Angle:");
        OLED_Printf(0, 12, OLED_6X8, "Kp:%05.3f", AnglePID.Kp);
        OLED_Printf(0, 20, OLED_6X8, "Ki:%05.3f", AnglePID.Ki);
        OLED_Printf(0, 28, OLED_6X8, "Kd:%05.3f", AnglePID.Kd);
        
        OLED_Printf(0, 40, OLED_6X8, "Tar:%04.0d", AnglePID.Target);
        OLED_Printf(0, 48, OLED_6X8, "Act:%04d", angle);
        OLED_Printf(0, 56, OLED_6X8, "Out:%04.0f", AnglePID.Out);
        
        LocationPID.Kp = RP_GetValue(1) / 4095.0 * 1;
        LocationPID.Ki = RP_GetValue(2) / 4095.0 * 1;
        LocationPID.Kd = RP_GetValue(3) / 4095.0 * 1;
        
        OLED_Printf(64, 0, OLED_6X8, "Location:");
        OLED_Printf(64, 12, OLED_6X8, "Kp:%05.3f", LocationPID.Kp);
        OLED_Printf(64, 20, OLED_6X8, "Ki:%05.3f", LocationPID.Ki);
        OLED_Printf(64, 28, OLED_6X8, "Kd:%05.3f", LocationPID.Kd);
                     
        OLED_Printf(64, 40, OLED_6X8, "Tar:%04.0d", LocationPID.Target);
        OLED_Printf(64, 48, OLED_6X8, "Act:%04d",   Location);
        OLED_Printf(64, 56, OLED_6X8, "Out:%04.0f", LocationPID.Out);
        
	}
}

void TIM1_UP_IRQHandler(void)
{
    static uint16_t angle0, angle1, angle2;
	if (TIM_GetITStatus(TIM1, TIM_IT_Update) == SET)
	{
		Key_Tick();
        
        Speed = Encoder_Get();
        Location += Speed;        
        angle = AD_JD_GetValue();

        if (RunState == 0)
        {
            Motor_SetPWM(0);
        }
        else if (RunState == 1)
        {
            Count++;
            if (Count > 40)
            {
                Count= 0;
                
                angle2 = angle1;
                angle1 = angle0;
                angle0 = angle;
                
                if (angle1 > angle0
                && angle1 > angle2
                && angle0 < CENTER_ANGLE - CENTER_RANGE
                && angle1 < CENTER_ANGLE - CENTER_RANGE
                && angle2 < CENTER_ANGLE - CENTER_RANGE) // 左侧检测
                {
                    RunState = 21;
                }
                
                if (angle1 < angle0
                && angle1 < angle2
                && angle0 > CENTER_ANGLE + CENTER_RANGE
                && angle1 > CENTER_ANGLE + CENTER_RANGE
                && angle2 > CENTER_ANGLE + CENTER_RANGE) // 右侧检测
                {
                    RunState = 31;
                }
                
                if (angle0 > CENTER_ANGLE - CENTER_RANGE
                 && angle0 < CENTER_ANGLE + CENTER_RANGE
                 && angle1 > CENTER_ANGLE - CENTER_RANGE
                 && angle1 < CENTER_ANGLE + CENTER_RANGE
                 && angle2 > CENTER_ANGLE - CENTER_RANGE
                 && angle2 < CENTER_ANGLE + CENTER_RANGE)
                {
                    RunState = 4;
                    AnglePID.ErrorInt = 0; // 由于内环（角速度环有Ki项）避免上次错误的误差影响本次起摆
                    Location = 0; // 让起摆位置为零位置
                }
            }
        }
        else if (RunState == 21)
        {
            Count0 = Time_CNT;
            Motor_SetPWM(Angle_PWM);
            RunState = 22;
            
        }
        else if (RunState == 22)
        {
            Count0--;
            if (Time_CNT == 0)
            {
                RunState = 23;
            }
        }
        else if (RunState == 23)
        {
            Count0 = Time_CNT;
            Motor_SetPWM(-Angle_PWM);
            RunState = 24;
        }
        else if (RunState == 24)
        {
            Count0--;
            if (Time_CNT == 0)
            {
                Motor_SetPWM(0);
                RunState = 23;
            }
        }
        else if (RunState == 31)
        {
            Count0 = Time_CNT;
            Motor_SetPWM(-Angle_PWM);
            RunState = 32;
        }
        else if (RunState == 32)
        {
            Count0--;
            if (Time_CNT == 0)
            {
                RunState = 33;
            }
        }
        else if (RunState == 33)
        {
            Count0 = Time_CNT;
            Motor_SetPWM(Angle_PWM);
            RunState = 34;
        }
        else if (RunState == 34)
        {
            Count0--;
            if (Time_CNT == 0)
            {
                Motor_SetPWM(0);
                RunState = 1;
            }
        }
        else if (RunState == 4) // 控制倒立摆运行
        {
            Count1++,Count2++;
            
            if (angle > CENTER_ANGLE + CENTER_RANGE || angle < CENTER_ANGLE - CENTER_RANGE) // 判断是否倾倒
            {
                RunState = 0;
            }
            if (Count1 >= 5) // 倒立摆需要快速响应，周期必须小
            {
                Count1 = 0;
                AnglePID.Actual = angle;
                PID_Update(&AnglePID);
                Motor_SetPWM(AnglePID.Out);
            }
            
            if (Count2 >= 50) // 位置控制相对来说没有那么强的响应要求
            {
                Count2 = 0;
                LocationPID.Actual = Location;
                PID_Update(&LocationPID);
                AnglePID.Target = CENTER_ANGLE + LocationPID.Out;
            }
        }
		TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
	}
}
